#include "rtk_planning_core.h"

RtkPlanningCore::RtkPlanningCore(const std::string filename) : filename_(filename),
                                                               planning_path2kd_(add_csv_rtk_path_),
                                                               index_(2 /*dim*/, planning_path2kd_, KDTreeSingleIndexAdaptorParams(10 /* max leaf */))
{
    add_csv_rtk_path_.read_csv_to_path(filename_);

    clock_t startTime, endTime;
    // construct a kd-tree index:
    startTime = clock(); //计时开始
    index_.buildIndex();
    endTime = clock(); //计时结束
    std::cout << "The tree build time is: " << (double)(endTime - startTime) / CLOCKS_PER_SEC << "s" << std::endl;
}
void RtkPlanningCore::searchNearstPoint(const std::vector<double> query_vector2d)
{
    double query_pt[2] = {query_vector2d[0], query_vector2d[1]};
    // do a knn search
    std::cout << "search point(current point) is " << query_vector2d[0] << " " << query_vector2d[1] << std::endl;
    clock_t startTime, endTime;
    startTime = clock(); //计时开始
    const size_t num_results = 1;
    nanoflann::KNNResultSet<double> resultSet(num_results);
    resultSet.init(&closet_point_num_, &closet_dist_sqr_);
    index_.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
    //index.knnSearch(query, indices, dists, num_results, mrpt_flann::SearchParams(10));
    std::cout << "closet_point_num_=" << closet_point_num_ << " closet_dist_sqr_=" << closet_dist_sqr_ << std::endl;
    endTime = clock(); //计时结束
    std::cout << "The tree search time is: " << (double)(endTime - startTime) / CLOCKS_PER_SEC << "s" << std::endl;
}
void RtkPlanningCore::pathBePublished(int num_of_front_point, int num_of_rear_point)
{

    add_csv_ros_path_.path_.path.clear();
    add_csv_ros_path_.path_.closet_point_num = num_of_rear_point;
    int total_num = add_csv_rtk_path_.path_.path.size();
    std::cout << "total_num is " << total_num << std::endl;
    for (int index = -num_of_rear_point; index < num_of_front_point; index++)
    {
        int real_index = index + closet_point_num_;
        if (real_index < 0)
        {
            real_index += total_num;
        }
        else if (real_index >= total_num)
        {
            real_index -= total_num;
        }
        else
        {
        }
        auto_msgs::planning_point temp_point;
        temp_point = add_csv_rtk_path_.path_.path[real_index];
        add_csv_ros_path_.path_.path.push_back(temp_point);
    }
    
}
//用于测试
/*int main()
{
    RtkPlanningCore rtk_planning("rtk_plannig.csv");
    std::vector<double> test_vector{0,0};
    rtk_planning.searchNearstPoint(test_vector);
    rtk_planning.pathBePublished(100,0);
    std::cout<<"path"<<std::endl;
    rtk_planning.add_csv_ros_path_.write_path_to_csv("planning_path.csv");
    
}*/